/*****************************************************************************
** Includes
*****************************************************************************/

#include "RobotCmdNode.hpp"
#include <QString>
#include <QVariantMap>
#include "CamCalCenter.h"
#include <QDebug>

/*****************************************************************************
** Namespaces
*****************************************************************************/
using namespace std::chrono_literals;
using std::placeholders::_1;

/*****************************************************************************
** Implementation
*****************************************************************************/

RobotCmdNode::RobotCmdNode(CamCalCenter* rspc)
    : Node("camcalcenter_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)), 
    m_rspc(rspc)
{
    m_pubCmd = this->create_publisher<pccmd_msg::msg::PcCmd>("PcCmdMsg",10);

    m_pubClawCmd = this->create_publisher<clawcmd_msg::msg::ClawCmd>("ClawCmd", 10);

    m_subParam = this->create_subscription<param_msg::msg::Param>(
        "ParamMsg", 10, std::bind(&RobotCmdNode::parammsg_callback, this,
        _1));

    m_subRsPos = this->create_subscription<rspos_msg::msg::RsPos>(
        "RsPos", 10, std::bind(&RobotCmdNode::rspos_callback, this, _1));

    m_subDlMsg = this->create_subscription<dl_msg::msg::DlMsg>(
        "DlMsg", 10, std::bind(&RobotCmdNode::dlmsg_callback, this, _1));

    m_subRsSwitch = this->create_subscription<rsswitch_msg::msg::RsSwitch>(
        "RsSwitch", 10, std::bind(&RobotCmdNode::rsswitch_callback, this, _1));

    m_pubMapText = this->create_publisher<maptext_msg::msg::MapText>("MapText", 10);
}

void RobotCmdNode::pubMapText(const std::string& name,
                              int size,
                              const QByteArray& data)
{
    auto cmd = maptext_msg::msg::MapText();
    cmd.name = name;
    cmd.size = size;
    memcpy(&cmd.data, data.data(), size);
    m_pubMapText->publish(cmd);
}

RobotCmdNode::~RobotCmdNode()
{
    rclcpp::shutdown();
}


void RobotCmdNode::clawControl(uint8_t claw_dir, uint8_t claw_force)
{
    auto clawcmd_msg = clawcmd_msg::msg::ClawCmd();
    clawcmd_msg.claw_dir = claw_dir;
    clawcmd_msg.claw_force = claw_force;
    m_pubClawCmd->publish(clawcmd_msg);

    RCLCPP_INFO(this->get_logger(),
                "CamCal Center RobotCmdNode claw control, claw dir: %d, claw force: %d",
                claw_dir, claw_force);
}

void RobotCmdNode::parammsg_callback(const param_msg::msg::Param::SharedPtr msg) const
{
    RCLCPP_INFO(this->get_logger(),
                "parammsg_callback cmdname: %s, params0: %s, params1: %s",
                msg->cmdname.c_str(), msg->params[0].c_str(), msg->params[1].c_str());

    std::string strCmdName = msg->cmdname;

    if ("setGenRParam" == strCmdName)
    {
        std::string str1 = msg->params[0];
        QString strSerial = QString::fromStdString(msg->params[1]);

        int nOperate = QString::fromStdString(str1).toInt();
        m_rspc->call_setGenRParam(nOperate, strSerial);
    }
    else if ("operate" == strCmdName)
    {
        qDebug() << __FUNCTION__ << " in operate";
        std::string str1 = msg->params[0];
        QString strSerial = QString::fromStdString(msg->params[1]);

        int nExecStep = QString::fromStdString(str1).toInt();
        m_rspc->call_operate(nExecStep, strSerial);
    }
    else if ("trigger_servercmd" == strCmdName)
    {
        int nLeftT = QString::fromStdString(msg->params[0]).toInt();
        int nRightT = QString::fromStdString(msg->params[1]).toInt();

        m_rspc->call_trigger(nLeftT, nRightT);
    }
}

void RobotCmdNode::rspos_callback(
    const rspos_msg::msg::RsPos::SharedPtr msg) const
{
    float fXL = msg->xl;
    float fYL = msg->yl;
    float fZL = msg->zl;
    float fLL = msg->ll;
    float fWL = msg->wl;
    float fHL = msg->hl;
    float fXR = msg->xr;
    float fYR = msg->yr;
    float fZR = msg->zr;
    float fLR = msg->lr;
    float fWR = msg->wr;
    float fHR = msg->hr;
    std::string strSerial = msg->serial;

    QVariantMap mapLeft, mapRight;
    mapLeft.insert("x", fXL);
    mapLeft.insert("y", fYL);
    mapLeft.insert("z", fZL);
    mapLeft.insert("l", fLL);
    mapLeft.insert("w", fWL);
    mapLeft.insert("h", fHL);

    mapRight.insert("x", fXR);
    mapRight.insert("y", fYR);
    mapRight.insert("z", fZR);
    mapRight.insert("l", fLR);
    mapRight.insert("w", fWR);
    mapRight.insert("h", fHR);

    QString serial = QString::fromStdString(strSerial);

    m_rspc->call_setSendRsPosData(mapLeft, mapRight, serial);
}

void RobotCmdNode::dlmsg_callback(const dl_msg::msg::DlMsg::SharedPtr msg) const
{
    double x = msg->x;
    double y = msg->y;
    double z = msg->z;
    double rx = msg->rx;
    double ry = msg->ry;
    double rz = msg->rz;
    double state = msg->state;

    qDebug() << "recv dl msg: " << x << " " << y << " " << z;

    m_rspc->call_setDLData(x, y, z, rx, ry, rz, state);
}

void RobotCmdNode::rsswitch_callback(
    const rsswitch_msg::msg::RsSwitch::SharedPtr msg) const
{
    qDebug() << "recv rsswitch: " << msg->state;
    m_rspc->call_setRsState(msg->state != 0);
}


void RobotCmdNode::sendCmd(const std::string& cmdname,
                           const std::string& cmdcontent)
{
    auto cmd_msg = pccmd_msg::msg::PcCmd();
    cmd_msg.cmdname = cmdname;
    cmd_msg.cmdcontent = cmdcontent;
    m_pubCmd->publish(cmd_msg);

    RCLCPP_INFO(this->get_logger(),
                "RobotCmdNode SendCmd, cmdname: %s, cmdcontent: %s",
                cmdname.c_str(), cmdcontent.c_str());
}
